#ifndef _UBT_STATE_TYPES_H_
#define _UBT_STATE_TYPES_H_
#include <string>

#include "std_msgs/msg/string.hpp"
#include "geometry_msgs/msg/pose2_d.hpp"
#include "ubt_state/ubt_state_config.h"

using namespace std;

namespace ubt_state
{
/*
状态转换图
初始状态: closed
正常转换:
	closed:首次收到开机信号，进initial
	initial:自检通过，第一次进initpose,后续进wait
		 自检失败,  进 nrefault
	initpose:收到初始定位结果(无论成败)，进入wait
	wait: 收到开始建图指令,新建map任务map,进run
	    : 收到开始任务指令,新建ex任务(普通或充电),进run
	run: 待当前任务状态为task_closed后，删除当前任务，进wait
	nrefault:停留此状态(此时当前任务可能还未结束)
异常转换:
	任何非nrefault状态:
		收到不可恢复故障信号,立即停止当前任务ITask.Stop,进nrefault状态
    任何非closed状态:
        收到关机信号，立即停止当前任务,进closed
备注:


任务:
	有类型，状态，阶段几个属性
	类型:
		有三种任务类型,map,gen普通，gen充电
	状态:
		有Idle,RunEx(Running,Pause,Obstacle), Stopping,Closed；状态转换图为
		Idle:
			收到Start调用后,成功进RunEx,失败进Closed
		RunEx:
			收到完成任务消息，进Closed
			收到Stop调用,进Stopping
		Stopping:
			收到完成任务消息或超时，进Closed
		Closed:
			无

		RunEx是复合状态，含三个子状态Running,Pause,Obstacle;
			收到Pause调用，为Pause；收到Continue调用后，退出Pause
			在非Pause状态下，有避障，为Obstacle,否则为Running

	阶段:
		因任务类型而不同;
		map任务:
			仅一个Mapping
	    gen普通任务:
	    	导航中，为Run
	    	动作中，为Action
		gen充电任务：
			上桩前，为Recharge
			其他，为Charge
*/

/*状态值*/
enum ubt_diagram_state
{
    ubt_diagram_closed = 0,//初始状态
    ubt_diagram_initial = 1,//硬件自检
    ubt_diagram_initpose = 2,//初始定位
    ubt_diagram_wait = 3,//空闲,不存在当前任务
    ubt_diagram_run = 4,//运行任务中
    ubt_diagram_nrfault = 5,//不可恢复故障,(当前任务可能未结束,关机属于该了故障)
};

/*任务类型*/
enum ubt_task_type
{
    ubt_task_type_ex = 0,
    ubt_task_type_map,
    ubt_task_type_nav,
    ubt_task_type_charge,
    ubt_task_type_action,
    ubt_task_type_adjust,
    ubt_task_type_chargeex,
    ubt_task_type_nav2,
};

/*任务状态*/
enum ubt_task_state
{
    ubt_task_state_idle = 0,
    ubt_task_state_pause = 1,
    ubt_task_state_obstacle = 2,
    ubt_task_state_running = 3,
    ubt_task_state_stopping = 4,
    ubt_task_state_closed = 5,
};

/*任务阶段*/
enum ubt_task_stage
{
    ubt_task_stage_mapping,
    ubt_task_stage_run,
    ubt_task_stage_action,
    ubt_task_stage_recharge,
    ubt_task_stage_charge,
    ubt_task_stage_recharge2,//上桩中
    ubt_task_stage_recharge_fail,//上桩超过三次失败
};

enum ubt_msg_type
{
    ubt_msg_type_current_pose,	//geometry_msgs::msg::Pose2D*
    ubt_msg_type_avoid_status,	//StateStatus.avoidStatus*
    ubt_msg_type_map_confidence, //StateStatus.curPoseCfd*
    ubt_msg_type_mb_status,		//StateStatus.mbStatus*
    ubt_msg_type_nav_limit, //NavTaskLimitIndex* 交通管制
};

typedef struct
{
    long time;
    std::string msg;
} TaskLogItem;

class ITask
{
  public:
    virtual ~ITask() {};

    virtual bool Start() = 0;
    virtual void Pause() = 0;
    virtual void Continue() = 0;
    virtual void Stop(int reason) = 0;
    virtual void OnTimer() = 0;
    /*接收到(公共类)消息,msgType取值见  ubt_msg_xxx*/
    virtual void OnMsg(int msgType, void *pvMsg) = 0;

    virtual ubt_task_type GetType() = 0;
    /*
    	获取状态: ubt_task_state
    */
    virtual ubt_task_state GetState() = 0;
    /*
        获取阶段 ubt_task_stage
    */
    virtual ubt_task_stage GetStage() = 0;

    virtual std::string GetActionType()=0;

    virtual void GetIndex(int &index,int & reportIndex)=0;

    virtual void GetStopInfo(int &stopReason,std::string &stopReasonDesc)=0;

    virtual void GetLogs(std::vector<TaskLogItem> &logs)=0;
};


typedef struct
{
    int mbVersion;//电机板版本
    int mbStatus;//电机板 急停状态 ，左轮/右轮/滚筒电机报警状态
#if (PLAT_USE == PLAT_U1000)
    int powerState;  //开关机状态
    int chargingState;//充电状态
    int chargingPile;//充电桩状态 charging_pile
    int chargingConnect;//充电连接状态
#else
    int pbStatus;//开关机/电池通讯状态
    int pbChargeStatus;//充电状态
#endif
    bool antcTrigged;//防撞条是否已触发
    bool breakOpened;//抱闸是否松开
    int liftState;//顶升状态
    int dragState;//牵引销状态
    int forkState; //货叉状态
    long rackTag;//货架tag
    geometry_msgs::msg::Pose2D rackPose;//货架位姿
    long rackPoseTime;//货架位姿时间
    int netStatus;//网络状态
    int laserAvoidStatus; //雷达避障状态avoid_status_value_xxx
    int rgbdAvoidStatus;//rgbd避障状态avoid_status_value_xxx
    int joystickStatus;	//手柄状态
    int curPerPower; //当前电量,0-100

    geometry_msgs::msg::Pose2D curPose; //当前位姿
    long curPoseTime; //当前位姿时间
    float curPoseCfd;  //当前位姿置信度
    long curPoseCfdTime; //当前位姿置信度时间
    int poseState;//pose_state_xxx
    long poseStateTime;//poseState时间
    geometry_msgs::msg::Pose2D odomPose; //里程计位姿
    long odomPoseTime;//里程计位姿时间
    float bleSpeed;//遥控最大线速度,单位 米/秒

    float expLength;//膨胀长度
    float expWidth;//膨胀宽度
    float expTheta;//膨胀时货架二维码的theta
    int nav2CheckTimeout;//nav2是否检测超时
    bool autoFindState;//自巡线状态
    bool isStandaloneMode; // 单机（空跑）

#ifdef UBT_LIFT_SUPPORT
    bool isUnderShelve;//在货架下方吗(且未顶起状态)
    geometry_msgs::msg::Pose2D shelvePose;//货架的位置
#endif
    int avoidHwStatus;//综合laserAvoidStatus和rgbdAvoidStatus后的 avoid_status_value_xxx
} StateStatus;
#define mb_status_flag_bluetooth_connected  0x01		//蓝牙连上
#define mb_status_flag_stop_alarm    0x02	//急停按下
#define mb_status_flag_leftwheel_alarm  0x04//左轮报警
#define mb_status_flag_rightwheel_alarm  0x08	//右轮报警
#define mb_status_flag_roller_alarm  0x10	//滚筒报警
#define mb_status_flag_nrfault 	0x1c	//leftwheel|rightwheel|roller

#if (PLAT_USE == PLAT_U1000)
enum power_state
{
    power_state_off,
    power_state_off_going,
    power_state_on_going,
    power_state_on
};
enum charging_state
{
    charging_state_idle = 0,//空闲
    charging_state_charging = 1,//充电
    charging_state_releasing = 2,//放电
    charging_state_full = 3,//满电
};
enum charging_pile
{
    charging_pile_off = 0,	/*不在线*/
    charging_pile_on = 1, 	/*在线*/
};
enum charging_connect
{
    charging_connect_null = 0,	/*未连接*/
    charging_connect_bed = 1, 	/*底座充*/
    charging_connect_wire = 2, 	/*线充*/
};

#else
#define pb_status_flag_poweroff 0x01
#define pb_status_flag_batteryexception 0x02
enum pb_charge_status
{
    pb_charge_status_idle = 0,
    pb_charge_status_charging = 1,
    pb_charge_status_err = 2,
    pb_charge_status_full = 3,
};
#endif

enum lift_state
{
    lift_state_unknown = -1, //未知
    lift_state_mid = 0,//中间
    lift_state_down = 1,//顶升在底部
    lift_state_up = 2,//顶升在顶部
};
enum drag_state
{
    drag_state_error = -2, //牵引销报错
    drag_state_unknown = -1, //未知
    drag_state_mid = 0,//牵引销在中间
    drag_state_down = 1,//牵引销在底部
    drag_state_up = 2,//牵引销在顶部
};
enum fork_state
{
    fork_state_error = -2,
    fork_state_unknown = -1,
    fork_state_down = 0,
    fork_state_mid = 1,
    fork_state_up = 2,
};

#define net_status_flag_wificonnected 0x01  //wifi已连接

#define avoid_status_value_none 0	//不避障
#define avoid_status_value_stop 1  	//急停
#define avoid_status_value_slow 2	//减速
#define avoid_status_value_warn 3	//报警
#define avoid_status_value_stopleft 4	//急停2(左侧)
#define avoid_status_value_stopright 5	//急停2(右侧)

#define joystick_status_flag_connected 0x01 //手柄连上

#define pose_state_idle         0  //等待地图
#define pose_state_syncmap      1  //同步地图
#define pose_state_waitloc      2  //等待定位
#define pose_state_locating     3  //定位中
#define pose_state_ok           4  //已定位


#define avoid_hw_mask_rgbd 0x02	//避障忽略rgbd

class ITaskCb
{
  public:
    virtual StateStatus *GetStateStatus() = 0;
    /*
    	taskEvent:取值为 UBT_TASK_EVENT_*_STR
    */
    virtual void  NotifyTaskEvent(std::string taskEvent) = 0;

    /*
    	任务状态或阶段发生变化,
    	当state=closed时，stateReason,stateReasonDesc有意义,
    	stateReason 取值ubt_task_result_xx( ok 正常结束,其他 失败结束)
    	注意 state=closed通知后,任务可能会被删除
    */
    virtual void NotifyTaskStateAndStage(int state, int stage, int stateReason, std::string stateReasonDesc) = 0;

};

/*复合状态值
  结合考虑ubt_diagram_state与当前任务的ubt_task_state,ubt_task_stage而定
  具体请参考CStateDiagram._RefreshState
*/
enum ubt_state_pub
{
    ubt_Close = 0, //初始状态
    ubt_Initial,//硬件自检 1
    ubt_Initpose,//初始定位 2
    ubt_Wait, //空闲 3
    ubt_Mapping, // 4
    ubt_Run, //运行中 5
    ubt_Action, // 6
    ubt_Recharge,	//回充(导航和上桩) 7
    ubt_Charge, 	//充电(充电和正常下桩) 8
    ubt_Obstacle,  //避障 9
    ubt_Pause,	//暂停 10
    ubt_Stop,	//急停按下 11
    ubt_Refault,  //可恢复故障 12
    ubt_NRefault,  //不可恢复故障(关机，电机异常等) 13
    ubt_Rmctl, 	//遥控模式  14
    ubt_ChargeManual, //手动充电  15
    ubt_PauseManual, //手动暂停 16
    ubt_WaitPose,	//待定位 17
    ubt_BreakOpened, //解抱闸 18
    ubt_Collide,  //碰撞中 19
    ubt_LiftUp,     //顶升中 20
    ubt_DragUp,     //牵引销顶升中 21
    ubt_PauseLimit, //到点暂停 22
    ubt_AutoFind, //自巡线 23
    ubt_Update, //更新中 24
    ubt_ShelfUp, //货叉升起 F1200 25
    Ubt_StandaloneMode, //单机（空跑）模式 26
};

enum ubt_task_result
{
    ubt_task_result_ok = 0,
    //公共
    //用户本体行为和系统故障类
    ubt_task_result_sys_antc = 1,
    ubt_task_result_usr_stopbtn = 2,
    ubt_task_result_usr_brakebtn = 3,
    ubt_task_result_sys_poweroff = 4,
    ubt_task_result_sys_nrfault = 5,
    ubt_task_result_sys_charge = 6,

    //系统异常类
    ubt_task_result_sys_bad_status = 10,
    ubt_task_result_sys_posefail = 11,
    ubt_task_result_mod_timeout = 12,

    //后台操作类
    ubt_task_result_req_bad_param = 100, //请求参数有错
    ubt_task_result_sys_mapupdate = 101,
    ubt_task_result_usr_cancel = 102,	// 用户取消--导航和任务都取消
    ubt_task_result_usr_dispatchcancel = 103,	// 调度取消--只取消导航，任务不取消
    ubt_task_result_navigation_cancel = 104, // 本体取消--只取消导航，任务不取消

    ubt_task_result_com_max = 9999,

    //导航
    ubt_task_result_nav_base = 10000,
    ubt_task_result_nav_abort = ubt_task_result_nav_base + 1,
    ubt_task_result_nav_aborte3m = ubt_task_result_nav_base + 2,
    ubt_task_result_nav_posejump = ubt_task_result_nav_base + 3,
    ubt_task_result_nav_trackfail = ubt_task_result_nav_base + 4,
    ubt_task_result_nav_max = ubt_task_result_nav_base + 9999,
    //动作
    ubt_task_result_action_base = 20000,
    ubt_task_result_action_abort = ubt_task_result_action_base + 1,
    ubt_task_result_action_fail = ubt_task_result_action_base + 2,
    ubt_task_result_action_cancel = ubt_task_result_action_base + 3,
    ubt_task_result_action_max = ubt_task_result_action_base + 9999,
    //充电
    ubt_task_result_charge_base = 30000,
    ubt_task_result_charge_abort = ubt_task_result_charge_base + 1,
    ubt_task_result_charge_max = ubt_task_result_charge_base + 9999,
    //微调
    ubt_task_result_adjust_base = 40000,
    ubt_task_result_adjust_abort = ubt_task_result_adjust_base,
    ubt_task_result_adjust_startfail = ubt_task_result_adjust_base + 1,
    ubt_task_result_adjust_timeout = ubt_task_result_adjust_base + 128,
    ubt_task_result_adjust_max = ubt_task_result_adjust_base + 9999,
};

typedef struct
{
    float stopDis;
    float slowDis;
    float warnDis;
    float sideDis;
} ObstacleParams;

}

#endif /*_UBT_STATE_TYPES_H_*/

